/**
 * This controller attempts to use a subsumption architecture to push balls to a
 * light source.
 * 
 * @date Nov 17, 2008
 * @author Malcolm Staudacher
 * 
 */
public class BallLightPusher extends Subsumption {

	/**
	 * Indicates that the robot wants to get in position to push the ball.
	 */
	private boolean m_positionerFlag;

	/**
	 * Indicates that the robot is already trying to get in position (since last
	 * cycle).
	 */
	private boolean m_positioning;

	/**
	 * The motor speeds determined by the positioner method.
	 */
	private int[] m_positionerOutput = { 0, 0 };


	public BallLightPusher() {
		setWaitTime(5);
		m_cruiserFlag = false;
		m_wcheckerFlag = false;
		m_bfinderFlag = false;
		m_bpusherFlag = false;
		m_positionerFlag = false;
		m_positioning = false;
		m_milage = 0;
	}


	/**
	 * Determines, which tasks output to use.
	 */
	@Override
	public void arbitrate() {

		if (m_cruiserFlag == true) {
			System.out.println("Cruise");
			setMotorSpeeds(m_cruiseOutput[0], m_cruiseOutput[1]);
		}

		if (m_bfinderFlag == true) {
			System.out.println("Ballfinder");
			setMotorSpeeds(m_bfinderOutput[0], m_bfinderOutput[1]);
		}

		if (m_bpusherFlag == true) {
			System.out.println("Ballpusher");
			setMotorSpeeds(m_bpusherOutput[0], m_bpusherOutput[1]);
		}

		if (m_wcheckerFlag == true) {
			System.out.println("Avoid");
			setMotorSpeeds(m_wcheckerOutput[0], m_wcheckerOutput[1]);
			this.sleep(1200);
		}

		if (m_positionerFlag == true) {
			System.out.println("Positioner");
			setMotorSpeeds(m_positionerOutput[0], m_positionerOutput[1]);
		}

	}


	/**
	 * Performs tasks in order of priority. Lower priority tasks will only be
	 * performed if higher priority tasks have no interest in controlling the
	 * robot.
	 */
	public void doWork() throws Exception {

		positioner();

		if (m_positionerFlag == false) {
			wallChecker();

			if (m_wcheckerFlag == false) {
				ballPusher1();

				if (m_bpusherFlag == false) {
					ballFinder1();

					if (m_bfinderFlag == false) {
						cruise();
					}

				}

			}

		}

		arbitrate();
		setMilage();
	}


	/**
	 * Locates balls which are detected by the sensors of the robot.
	 */
	public void ballFinder1() {
		getSensorValues();

		if ((s2 > LOW_THRESHOLD || s3 > LOW_THRESHOLD) && nothingLeft()
				&& nothingRight() && getLightValue(1) > MID_LIGHTTHRESHOLD
				&& getLightValue(4) > MID_LIGHTTHRESHOLD) {
			// move towards ball
			if (s2 > s3) {
				m_bfinderOutput[0] = 0;
				m_bfinderOutput[1] = 2;
			} else if (s3 > s2) {
				m_bfinderOutput[0] = 2;
				m_bfinderOutput[1] = 0;
			} else {
				m_bfinderOutput[0] = 2;
				m_bfinderOutput[1] = 2;
			}
			m_bfinderFlag = true;
			// Ball at Sensor s0
		} else if (s0 > LOW_THRESHOLD && s1 < LOW_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -4;
			m_bfinderOutput[1] = 4;
			m_bfinderFlag = true;
			// Ball at Sensor s5
		} else if (s5 > LOW_THRESHOLD && s4 < LOW_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 4;
			m_bfinderOutput[1] = -4;
			m_bfinderFlag = true;
			// Ball at Sensor s1
		} else if (s1 > LOW_THRESHOLD && s0 < LOW_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -3;
			m_bfinderOutput[1] = 3;
			m_bfinderFlag = true;
			// Ball at Sensor s4
		} else if (s4 > LOW_THRESHOLD && s5 < LOW_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 3;
			m_bfinderOutput[1] = -3;
			m_bfinderFlag = true;

		} else if (s0 > LOW_THRESHOLD && s1 < MID_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -4;
			m_bfinderOutput[1] = 4;
			m_bfinderFlag = true;
			// Ball at Sensor s5 + s4
		} else if (s5 > LOW_THRESHOLD && s4 < MID_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 4;
			m_bfinderOutput[1] = -4;
			m_bfinderFlag = true;
			// Ball at Sensor s1 + s0
		} else if (s1 > LOW_THRESHOLD && s0 < MID_THRESHOLD && nothingInFront()
				&& nothingRight()) {
			m_bfinderOutput[0] = -3;
			m_bfinderOutput[1] = 3;
			m_bfinderFlag = true;
			// Ball at Sensor s4 + s5
		} else if (s4 > LOW_THRESHOLD && s5 < MID_THRESHOLD && nothingInFront()
				&& nothingLeft()) {
			m_bfinderOutput[0] = 3;
			m_bfinderOutput[1] = -3;
			m_bfinderFlag = true;

		} else {
			m_bfinderFlag = false;
		}
	}


	/**
	 * Pushes the ball straight ahead.
	 */
	public void ballPusher1() {

		if (m_positionerFlag == false) {
			getSensorValues();

			// if ball is in front and the robot is still moving, push
			if ((s2 >= SENSOR_MAX && s3 >= SENSOR_MAX)
					&& (m_milage != getActualMilage())
					&& getLightValue(1) > MID_LIGHTTHRESHOLD 
					&& getLightValue(4) > MID_LIGHTTHRESHOLD) {
				m_bpusherOutput[0] = 8;
				m_bpusherOutput[1] = 8;
				m_bpusherFlag = true;
			} else {
				m_bpusherFlag = false;
			}
			// setMilage();
		}

	}


	/**
	 * Tries to position the robot, so that the ball is between the robot and
	 * the light.
	 */
	public void positioner() {

		// only do this if the ball is being pushed or if the robot is already
		// trying to get into the right position
		if ((m_bpusherFlag == true && m_positionerFlag == false)
				|| m_positioning == true) {
			int minSensor = -1;
			int min = 0;

			// determine min value
			for (int i = 0; i < 8; i++) {

				if (getLightValue(i) < min) {
					min = getLightValue(i);
					minSensor = i;
				}

			}

			if (m_positioning == false) {

				// make sure the robot is going towards the light
				if (minSensor != 2 && minSensor != 3) {
					m_positionerFlag = true;
					m_positioning = true;
					System.out.println("going around ball");
					// turn away from the light (to go around the ball)
					if (minSensor == 0 || minSensor == 1) {
						m_positionerOutput[0] = -6;
						m_positionerOutput[1] = -1;
					} else if (minSensor == 4 || minSensor == 5) {
						m_positionerOutput[0] = -1;
						m_positionerOutput[1] = -6;
					} else {
						m_positionerOutput[0] = 9;
						m_positionerOutput[1] = -9;
					}

				}

			} else {
				System.out.println("turn back to ball");
				// m_positioning = false;
				int maxDist = 0;
				int maxIndex = -1;
				getSensorValues();
				setMilage();

				// determine max value
				for (int i = 0; i < 8; i++) {

					if (getDistanceValue(i) > maxDist) {
						maxDist = getDistanceValue(i);
						maxIndex = i;
					}

				}

				// turn back towards the ball
				if (maxIndex == 0) {
					m_positionerOutput[0] = -9;
					m_positionerOutput[1] = -2;
				} else if (maxIndex == 5) {
					m_positionerOutput[0] = -2;
					m_positionerOutput[1] = -9;
				} else if (maxIndex == 2 || maxIndex == 3) {
					m_positionerOutput[0] = 9;
					m_positionerOutput[1] = 9;
					m_positioning = false;
				} else {
					m_positionerOutput[0] = 5;
					m_positionerOutput[1] = 5;
				}

			}

		} else {
			m_positionerFlag = false;
		}

	}

}
